/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#include <global_planner/planner_core.h>
#include <pluginlib/class_list_macros.hpp>
#include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_2d.h>

#include <global_planner/dijkstra.h>
#include <global_planner/astar.h>
#include <global_planner/grid_path.h>
#include <global_planner/gradient_path.h>
#include <global_planner/quadratic_calculator.h>

//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

namespace global_planner
{

void GlobalPlanner::outlineMap(unsigned char *costarr, int nx, int ny, unsigned char value)
{
    unsigned char *pc = costarr;
    for (int i = 0; i < nx; i++) *pc++ = value;
    pc = costarr + (ny - 1) * nx;
    for (int i = 0; i < nx; i++) *pc++ = value;
    pc = costarr;
    for (int i = 0; i < ny; i++, pc += nx) *pc = value;
    pc = costarr + nx - 1;
    for (int i = 0; i < ny; i++, pc += nx) *pc = value;
}

GlobalPlanner::GlobalPlanner() :
        costmap_(NULL), initialized_(false), allow_unknown_(true),
        p_calc_(NULL), planner_(NULL), path_maker_(NULL), orientation_filter_(NULL),
        potential_array_(NULL) {}

GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id) : GlobalPlanner()
{
    // initialize the planner
    initialize(name, costmap, frame_id);
}

GlobalPlanner::~GlobalPlanner()
{
    delete p_calc_;
    delete planner_;
    delete path_maker_;
    delete dsrv_;
}

void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
{
    initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
}

void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id)
{
    if (!initialized_)
    {
        ros::NodeHandle private_nh("~/" + name);
        costmap_ = costmap;
        frame_id_ = frame_id;
        unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
        // 如果你想要让global_planner跟之前的navfn版本效果一样，就设true，所以不建议设置为true。
        private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
        if (!old_navfn_behavior_) convert_offset_ = 0.5;
        else convert_offset_ = 0.0;
        bool use_quadratic;
        // 设置为true，将使用二次函数近似函数;否则,使用更加简单的计算方式，这样节省硬件计算资源。
        private_nh.param("use_quadratic", use_quadratic, true);
        // 根据选择new出对应的p_calc_实例. 计算“一个点”的可行性
        if (use_quadratic) p_calc_ = new QuadraticCalculator(cx, cy); // 使用二次差值近似的potential
        else p_calc_ = new PotentialCalculator(cx, cy);
        bool use_dijkstra;
        private_nh.param("use_dijkstra", use_dijkstra, true);
        // 根据选择new出对应的planner实例. 计算“所有”的可行点
        if (use_dijkstra)
        {
            // dijkstra算法
            DijkstraExpansion *de = new DijkstraExpansion(p_calc_, cx, cy);
            if (!old_navfn_behavior_) de->setPreciseStart(true);
            planner_ = de;
        }
            // A*算法
        else planner_ = new AStarExpansion(p_calc_, cx, cy);
        bool use_grid_path;
        private_nh.param("use_grid_path", use_grid_path, false);
        // 路径方法，new出path_maker_实例。从可行点中提取路径
        // 栅格路径，从终点开始找上下或左右4个中最小的栅格直到起点
        if (use_grid_path) path_maker_ = new GridPath(p_calc_);
            // 梯度路径，从周围八个栅格中找到下降梯度最大的点
        else path_maker_ = new GradientPath(p_calc_);
        orientation_filter_ = new OrientationFilter();
        plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
        potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
        private_nh.param("allow_unknown", allow_unknown_, true);
        planner_->setHasUnknown(allow_unknown_);
        private_nh.param("planner_window_x", planner_window_x_, 0.0);
        private_nh.param("planner_window_y", planner_window_y_, 0.0);
        // 当设置的目的地被障碍物占据时,需要以该参数为半径寻找到最近的点作为新目的地点。
        private_nh.param("default_tolerance", default_tolerance_, 0.0);
        private_nh.param("publish_scale", publish_scale_, 100);
        // 用致命的障碍勾勒出全局代价地图。对于非静态（滚动窗口）全局代价地图的使用，需要将其设置为false
        private_nh.param("outline_map", outline_map_, true);
        make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);
        dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
        dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb =
                [this](auto &config, auto level) { reconfigureCB(config, level); };
        dsrv_->setCallback(cb);
        initialized_ = true;
    }
    else ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
}

void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig &config, uint32_t level)
{
    planner_->setLethalCost(config.lethal_cost);
    path_maker_->setLethalCost(config.lethal_cost);
    planner_->setNeutralCost(config.neutral_cost);
    planner_->setFactor(config.cost_factor);
    publish_potential_ = config.publish_potential;
    orientation_filter_->setMode(config.orientation_mode);
    orientation_filter_->setWindowSize(config.orientation_window_size);
}

void GlobalPlanner::clearRobotCell(unsigned int mx, unsigned int my)
{
    if (!initialized_)
    {
        ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return;
    }
    //set the associated costs in the cost map to be free
    costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);
}

bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
{
    makePlan(req.start, req.goal, resp.plan.poses);
    resp.plan.header.stamp = ros::Time::now();
    resp.plan.header.frame_id = frame_id_;
    return true;
}

void GlobalPlanner::mapToWorld(double mx, double my, double &wx, double &wy)
{
    wx = costmap_->getOriginX() + (mx + convert_offset_) * costmap_->getResolution();
    wy = costmap_->getOriginY() + (my + convert_offset_) * costmap_->getResolution();
}

bool GlobalPlanner::worldToMap(double wx, double wy, double &mx, double &my)
{
    double origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY();
    double resolution = costmap_->getResolution();
    if (wx < origin_x || wy < origin_y) return false;
    mx = (wx - origin_x) / resolution - convert_offset_;
    my = (wy - origin_y) / resolution - convert_offset_;
    return mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY();
}

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
                             std::vector<geometry_msgs::PoseStamped> &plan)
{
    return makePlan(start, goal, default_tolerance_, plan);
}

// 两个步骤完成路径的生成：
// 1. 计算“一个点”的可行性 —— p_calc_: PotentialCalculator::calculatePotential(),QuadraticCalculator::calculatePotential()
// 2. 计算可行点矩阵potential_array (planner_: DijkstraExpansion::calculatePotentials(),AStarExpansion::calculatePotentials())
// 3. 从可行点矩阵中提取路径plan (path_maker_: GridPath::getPath(),GradientPath::getPath())）
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
                             double tolerance, std::vector<geometry_msgs::PoseStamped> &plan)
{
    boost::mutex::scoped_lock lock(mutex_);
    // step 1: 是否已经初始化
    if (!initialized_)
    {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return false;
    }
    // 清空路径clear the plan, just in case
    plan.clear();
    ros::NodeHandle n;
    std::string global_frame = frame_id_;
    // step 2.1 目标点的坐标系应该和全局坐标系一致
    //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
    if (goal.header.frame_id != global_frame)
    {
        ROS_ERROR("The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.",
                global_frame.c_str(), goal.header.frame_id.c_str());
        return false;
    }
    // step 2.2 起始点的坐标系应该和全局坐标系一致
    if (start.header.frame_id != global_frame)
    {
        ROS_ERROR("The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.",
                global_frame.c_str(), start.header.frame_id.c_str());
        return false;
    }
    double wx = start.pose.position.x;
    double wy = start.pose.position.y;
    unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
    double start_x, start_y, goal_x, goal_y;
    // step 3 判断起始点和目标点是否超出了全局代价地图的范围
    if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i))
    {
        ROS_WARN_THROTTLE(1.0, "The robot's start position is off the global costmap. "
                               "Planning will always fail, are you sure the robot has been properly localized?");
        return false;
    }
    if (old_navfn_behavior_)
    {
        start_x = start_x_i;
        start_y = start_y_i;
    }
    else worldToMap(wx, wy, start_x, start_y);
    wx = goal.pose.position.x;
    wy = goal.pose.position.y;
    if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i))
    {
        ROS_WARN_THROTTLE(1.0, "The goal sent to the global planner is off the global costmap. "
                               "Planning will always fail to this goal.");
        return false;
    }
    if (old_navfn_behavior_)
    {
        goal_x = goal_x_i;
        goal_y = goal_y_i;
    }
    else worldToMap(wx, wy, goal_x, goal_y);
    // step 4 清除起始单元格，它不可能是障碍物
    //clear the starting cell within the costmap because we know it can't be an obstacle
    clearRobotCell(start_x_i, start_y_i);
    int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
    // step 5 确保global_planner用的数组大小和地图一致
    //make sure to resize the underlying array that Navfn uses
    p_calc_->setSize(nx, ny);
    planner_->setSize(nx, ny);
    path_maker_->setSize(nx, ny);
    // potential_array_ 起始点到当前点的cost
    potential_array_ = new float[nx * ny];
    // 将costmap的四周(边界)变为LETHAL_OBSTACLE
    if (outline_map_) outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
    // step 6 核心步骤， 计算出potential_array_,寻找所有的可行点。
    bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                     nx * ny * 2, potential_array_);
    if (!old_navfn_behavior_) planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
    if (publish_potential_) publishPotential(potential_array_);
    // 判断calculatePotentials()函数是否成功找到可行点矩阵potential_array_
    if (found_legal)
    {
        // step 7 提取全局路径，用path_maker_->getPath得到路径
        if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan))
        {
            // 更新目标点的时间戳，和路径的其他点时间戳一致
            //make sure the goal we push on has the same timestamp as the rest of the plan
            geometry_msgs::PoseStamped goal_copy = goal;
            goal_copy.header.stamp = ros::Time::now();
            plan.push_back(goal_copy);
        }
        else ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
    }
    else ROS_ERROR_THROTTLE(5.0, "Failed to get a plan.");
    // step 8 给路径添加方向  add orientations if needed
    orientation_filter_->processPath(start, plan);
    // 发布路径和可视化 publish the plan for visualization purposes
    publishPlan(plan);
    delete[] potential_array_;
    return !plan.empty();
}

void GlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped> &path)
{
    if (!initialized_)
    {
        ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return;
    }
    //create a message for the plan
    nav_msgs::Path gui_path;
    gui_path.poses.resize(path.size());
    gui_path.header.frame_id = frame_id_;
    gui_path.header.stamp = ros::Time::now();
    // Extract the plan in world co-ordinates, we assume the path is all in the same frame
    for (unsigned int i = 0; i < path.size(); i++) gui_path.poses[i] = path[i];
    plan_pub_.publish(gui_path);
}

bool GlobalPlanner::getPlanFromPotential(double start_x, double start_y, double goal_x, double goal_y,
                                         const geometry_msgs::PoseStamped &goal,
                                         std::vector<geometry_msgs::PoseStamped> &plan)
{
    if (!initialized_)
    {
        ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return false;
    }
    std::string global_frame = frame_id_;
    // 清空路径 clear the plan, just in case
    plan.clear();
    std::vector<std::pair<float, float> > path;
    // 这个path的点只有在map中的位置信息(x,y)
    if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path))
    {
        ROS_ERROR("NO PATH!");
        return false;
    }
    ros::Time plan_time = ros::Time::now();
    // 将path中每个点转换到world下，方向信息还没加入，这里统一设为零，然后依次存储到plan中
    for (int i = path.size() - 1; i >= 0; i--)
    {
        std::pair<float, float> point = path[i];
        // 把map的全局路径转换到world下 convert the plan to world coordinates
        double world_x, world_y;
        mapToWorld(point.first, point.second, world_x, world_y);
        geometry_msgs::PoseStamped pose;
        pose.header.stamp = plan_time;
        pose.header.frame_id = global_frame;
        pose.pose.position.x = world_x;
        pose.pose.position.y = world_y;
        pose.pose.position.z = 0.0;
        pose.pose.orientation.x = 0.0;
        pose.pose.orientation.y = 0.0;
        pose.pose.orientation.z = 0.0;
        pose.pose.orientation.w = 1.0;
        plan.push_back(pose);
    }
    if (old_navfn_behavior_) plan.push_back(goal);
    return !plan.empty();
}

void GlobalPlanner::publishPotential(float *potential)
{
    int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
    double resolution = costmap_->getResolution();
    nav_msgs::OccupancyGrid grid;
    // Publish Whole Grid
    grid.header.frame_id = frame_id_;
    grid.header.stamp = ros::Time::now();
    grid.info.resolution = resolution;
    grid.info.width = nx;
    grid.info.height = ny;
    double wx, wy;
    costmap_->mapToWorld(0, 0, wx, wy);
    grid.info.origin.position.x = wx - resolution / 2;
    grid.info.origin.position.y = wy - resolution / 2;
    grid.info.origin.position.z = 0.0;
    grid.info.origin.orientation.w = 1.0;
    grid.data.resize(nx * ny);
    float max = 0.0;
    for (unsigned int i = 0; i < grid.data.size(); i++)
    {
        float potential = potential_array_[i];
        if (potential < POT_HIGH)
        {
            if (potential > max) max = potential;
        }
    }
    for (unsigned int i = 0; i < grid.data.size(); i++)
    {
        if (potential_array_[i] >= POT_HIGH) grid.data[i] = -1;
        else
        {
            if (fabs(max) < DBL_EPSILON) grid.data[i] = -1;
            else grid.data[i] = potential_array_[i] * publish_scale_ / max;
        }
    }
    potential_pub_.publish(grid);
}

} //end namespace global_planner
